/*
 * motor_task.c
 *
 *  Created on: Oct 12, 2024
 *      Author: Jinming
 */
#include <string.h>
#include "motor_task.h"
#include "cmsis_os.h"
#include "task_info.h"
#include "can1.h"
#include "can2.h"
#include "host.h"

#define TAG		"motorTask"

MotorAttr_t g_tMotorAttr = {              //电机基本属性配置
    .ratedTorque = 7.7f,
	.ratedCurrent = 9.6f,
    .maxSpeed = 193.0f,
    .encoderResolution = ENCODER_PLUSE_ROUND,
    .reductionRatio = 54.4,
    .maxAcc = 2000.0f,               //最大加速度,RPM/s
    .maxDec = 2000.0f,               //最大减速度,RPM/s
    .stopDec = 2000.0f,              //快停减速度,RPM/s
    .tffAccDec = 20.0f,              //转矩变化率,Nm/s
	.torqueCoeff = 0.0217f,			//转矩常数,Nm/A
};

MiMotorAttr_t g_tMiMotorAttr = {              //小米电机基本属性配置
    .ratedTorque = 4.0f,
	.maxTorque = 12.0f,
	.ratedCurrent = 6.5f,
	.maxCurrent = 23.0f,
    .maxSpeed = 286.0f,
    .encoderResolution = 1,
    .reductionRatio = 1,
    .maxAcc = 2000.0f,               //最大加速度,RPM/s
    .maxDec = 2000.0f,               //最大减速度,RPM/s
    .stopDec = 2000.0f,              //快停减速度,RPM/s
    .tffAccDec = 20.0f,              //转矩变化率,Nm/s
	.torqueCoeff = 0.87f,			//转矩常数,Nm/A
};

static Motor_t g_tMotor[BO_MOTOR_NUM] =  		//博铭维四个电机
{
	[0] = {.type = MOTOR_TYPE_BOMINGWEI,
		   .busGroup = MOTOR_BUS_GROUP_0,
		   .canId = 0x601, .isExist = FALSE,
		   .motor.send    = can2_std_write,
		   .motor.receive = can2_read,
		   .motor.clear = can2_rx_clear
	},
	[1] = {.type = MOTOR_TYPE_BOMINGWEI,
		   .busGroup = MOTOR_BUS_GROUP_0,
		   .canId = 0x602, .isExist = FALSE,
		   .motor.send    = can2_std_write,
		   .motor.receive = can2_read,
		   .motor.clear = can2_rx_clear
	},
	[2] = {.type = MOTOR_TYPE_BOMINGWEI,
		   .busGroup = MOTOR_BUS_GROUP_1,
		   .canId = 0x601, .isExist = FALSE,
		   .motor.send = can1_std_write,
		   .motor.receive = can1_read,
		   .motor.clear = can1_rx_clear
	},
	[3] = {.type = MOTOR_TYPE_BOMINGWEI,
		   .busGroup = MOTOR_BUS_GROUP_1,
		   .canId = 0x602, .isExist = FALSE,
		   .motor.send    = can1_std_write,
		   .motor.receive = can1_read,
		   .motor.clear = can1_rx_clear
	},
};

static MiMotor_t g_tMiMotor[MI_MOTOR_NUM] =  		//2个小米电机
{
	[0] = {.type = MOTOR_TYPE_XIAOMI,
		   .busGroup = MOTOR_BUS_GROUP_0,
		   .canId = 0x16, .isExist = FALSE,
		   .motor.send    = can2_ext_write,
		   .motor.receive = can2_read,
		   .motor.clear = can2_rx_clear
	},
	[1] = {.type = MOTOR_TYPE_XIAOMI,
		   .busGroup = MOTOR_BUS_GROUP_1,
		   .canId = 0x16, .isExist = FALSE,
		   .motor.send    = can1_ext_write,
		   .motor.receive = can1_read,
		   .motor.clear = can1_rx_clear
	},
};

const MotorInfo_t defaultMotorInfo =
{
	.p = 0,
	.v = 0,
	.t = 0,
	.temperature = 0,
	.errorcode = 0xFFFF,
};

static ActiveMotor_t g_tActiveMotor[MOTOR_SUM];
static uint8_t g_cActiveMotorSum = 0;			//激活的电机总数

osThreadId_t g_tMotorTaskHandle = NULL;
const osThreadAttr_t g_tMotorTaskAttr = {
  .name = MOTOR_TASK_NAME,
  .stack_size = 128 * 8,
  .priority = (osPriority_t) MOTOR_TASK_PRIO,
};

osMessageQueueId_t g_tMotorQueue;
const osMessageQueueAttr_t g_tMotorQueueAttr = {
  .name = "motor"
};

osMutexId_t g_tActiveMotorMutex = NULL;
const osMutexAttr_t g_tActiveMotorMutexAttr = {
  .name = "ActiveMotor"
};

void motor_info_query_interrupt(void)
{
	uint8_t message[2];

	message[0] = MOTOR_MSG_GET_INFO;
	osMessageQueuePut(g_tMotorQueue, message, 0, 0);
}

void motor_dev_update(void)
{
	uint8_t message[2];

	message[0] = MOTOR_MSG_UPDATE_DEV;
	osMessageQueuePut(g_tMotorQueue, message, 0, 2);
}

void host_command_to_motor(void)
{
	uint8_t message[2];

	message[0] = MOTOR_MSG_SEND_COMMAND;
	osMessageQueuePut(g_tMotorQueue, message, 0, 2);
}

void host_lost_to_motor(void)
{
	uint8_t message[2];

	message[0] = MOTOR_MSG_HOST_LOST;
	osMessageQueuePut(g_tMotorQueue, message, 0, 0);
}

void motor_control_mode(uint8_t motorId, uint8_t mode)
{
	uint8_t curMode;

	for(uint8_t i = 0; i < g_cActiveMotorSum; i++)
	{
		if(g_tActiveMotor[i].index == motorId)
		{
			if(g_tActiveMotor[i].type == MOTOR_TYPE_BOMINGWEI)
			{
				curMode = motor_curMode_get(g_tActiveMotor[i].pMotorHandle);
				if(curMode == mode)
				{
					return;
				}
				motor_mode_change(&((Motor_t *)g_tActiveMotor[i].pMotorHandle)->motor, mode);
			}
			else if(g_tActiveMotor[i].type == MOTOR_TYPE_XIAOMI)
			{
				curMode = mi_motor_curMode_get(&((MiMotor_t *)g_tActiveMotor[i].pMotorHandle)->motor);
				if(curMode == mode)
				{
					return;
				}
				mi_motor_mode_change(&((MiMotor_t *)g_tActiveMotor[i].pMotorHandle)->motor, mode);
			}
		}
	}
}

void motor_task_torque_set(uint8_t motorId, float torque)
{
	for(uint8_t i = 0; i < g_cActiveMotorSum; i++)
	{
		if(g_tActiveMotor[i].index == motorId)
		{
			if(g_tActiveMotor[i].type == MOTOR_TYPE_BOMINGWEI)
			{
				if(motor_torque_set(&((Motor_t *)g_tActiveMotor[i].pMotorHandle)->motor, torque))
				{
					LOGE(TAG, "set torque error");
				}
				else
				{
					LOGD(TAG, "set torque ok");
				}
			}
			else if(g_tActiveMotor[i].type == MOTOR_TYPE_XIAOMI)
			{
				if(mi_motor_torque_set(&((MiMotor_t *)g_tActiveMotor[i].pMotorHandle)->motor, torque))
				{
					LOGE(TAG, "set torque error");
				}
				else
				{
					LOGD(TAG, "set torque ok");
				}
			}
		}
	}
}

void motor_task_speed_set(uint8_t motorId, float speed)
{
	for(uint8_t i = 0; i < g_cActiveMotorSum; i++)
	{
		if(g_tActiveMotor[i].index == motorId)
		{
			if(g_tActiveMotor[i].type == MOTOR_TYPE_BOMINGWEI)
			{
				if(motor_speed_set(&((Motor_t *)g_tActiveMotor[i].pMotorHandle)->motor, speed))
				{
					LOGE(TAG, "set speed error");
				}
				else
				{
					LOGD(TAG, "set speed ok");
				}
			}
			else if(g_tActiveMotor[i].type == MOTOR_TYPE_XIAOMI)
			{
				if(mi_motor_speed_set(&((MiMotor_t *)g_tActiveMotor[i].pMotorHandle)->motor, speed))
				{
					LOGE(TAG, "set speed error");
				}
				else
				{
					LOGD(TAG, "set speed ok");
				}
			}
		}
	}
}

void motor_task_position_set(uint8_t motorId, float pos)
{
	for(uint8_t i = 0; i < g_cActiveMotorSum; i++)
	{
		if(g_tActiveMotor[i].index == motorId)
		{
			if(g_tActiveMotor[i].type == MOTOR_TYPE_XIAOMI)
			{
				if(mi_motor_position_set(&((MiMotor_t *)g_tActiveMotor[i].pMotorHandle)->motor, pos))
				{
					LOGE(TAG, "set pos error");
				}
				else
				{
					LOGD(TAG, "set pos ok");
				}
			}
		}
	}
}

void motor_task_runctrl_set(uint8_t motorId, float pos, float speed, float kp, float ki)
{
	for(uint8_t i = 0; i < g_cActiveMotorSum; i++)
	{
		if(g_tActiveMotor[i].index == motorId)
		{
			if(g_tActiveMotor[i].type == MOTOR_TYPE_XIAOMI)
			{
				if(mi_motor_runctrl_param_set(&((MiMotor_t *)g_tActiveMotor[i].pMotorHandle)->motor, \
						pos, speed, kp, ki))
				{
					LOGE(TAG, "set runctrl params error");
				}
				else
				{
					LOGD(TAG, "set runctrl params ok");
				}
			}
		}
	}
}

void motor_stop(uint8_t motorId)
{
	if(motor_Idle(&g_tMotor[motorId].motor))
	{
		LOGE(TAG, "motor stop error");
	}
	else
	{
		LOGD(TAG, "motor stop");
	}
}

static void bo_motor_info_get(Motor_t *motor)
{
	int ret;
	float tmp;

	if(motor->isExist == FALSE)
	{
		motor->info.errorcode = 0xFFFF;		//未接入
		return;
	}
	else
	{
		motor->info.errorcode = motor->motor.errCode;
	}

	ret = motor_position_get(&motor->motor, &tmp);
	if(ret)
	{
		tmp = 0;
		motor->lostCnt++;
	}
	motor->info.p = tmp * 1000;

	ret = motor_speed_get(&motor->motor, &tmp);
	if(ret)
	{
		tmp = 0;
		motor->lostCnt++;
	}
	motor->info.v = tmp * 10000;

	ret = motor_current_get(&motor->motor, &tmp);
	if(ret)
	{
		tmp = 0;
		motor->lostCnt++;
	}
	motor->info.t = tmp * 10000;
}

static void mi_motor_info_get(MiMotor_t *motor)
{
	int ret;
	float tmp;
	int16_t temp;

	if(motor->isExist == FALSE)
	{
		motor->info.errorcode = 0xFFFF;		//未接入
		return;
	}
	else
	{
		motor->info.errorcode = motor->motor.errCode;
	}

	ret = mi_motor_limit_torque_set(&motor->motor, motor->motor.tMotorAttr.maxTorque);
	if(ret)			//数据读取出错
	{
		motor->lostCnt++;
		motor->info.p = 0;
		motor->info.v = 0;
		motor->info.t = 0;
	}
	else
	{
		motor->lostCnt = 0;
		mi_motor_position_get(&motor->motor, &tmp);
		motor->info.p = tmp * 1000;
		mi_motor_speed_get(&motor->motor, &tmp);
		motor->info.v = tmp * 10000;
		mi_motor_current_get(&motor->motor, &tmp);
		motor->info.t = tmp * 10000;
		mi_motor_temperature_get(&motor->motor, &temp);
		motor->info.temperature = temp;
	}
}

static void motor_info_get(void)
{
	uint8_t aMark[MOTOR_SUM] = {0, 0, 0, 0};

	for(uint8_t i = 0; i < g_cActiveMotorSum; i++)
	{
		if(g_tActiveMotor[i].type == MOTOR_TYPE_BOMINGWEI)
		{
			aMark[g_tActiveMotor[i].index] = 1;
			bo_motor_info_get(g_tActiveMotor[i].pMotorHandle);
			host_to_spi_sync(&((Motor_t *)g_tActiveMotor[i].pMotorHandle)->info, g_tActiveMotor[i].index);
		}
		else if(g_tActiveMotor[i].type == MOTOR_TYPE_XIAOMI)
		{
			aMark[g_tActiveMotor[i].index] = 1;
			mi_motor_info_get(g_tActiveMotor[i].pMotorHandle);
			host_to_spi_sync(&((MiMotor_t *)g_tActiveMotor[i].pMotorHandle)->info, g_tActiveMotor[i].index);
		}
	}

	for(uint8_t i = 0; i < MOTOR_SUM; i++)
	{
		if(aMark[i] == 0)
		{
			host_to_spi_sync((MotorInfo_t *)&defaultMotorInfo, i);
		}
	}
}

static void active_motor_sort(ActiveMotor_t *pList, uint8_t count)
{
	ActiveMotor_t temp;
	uint8_t index = 0;
	//排序
	for(uint8_t i = 0; i < count; i++)
	{
		for(uint8_t j = i+1; j < count; j++)
		{
			if((pList[i].busGroup > pList[j].busGroup) ||
			   ((pList[i].busGroup == pList[j].busGroup) && (pList[i].type > pList[j].type)))
			{
				memcpy(&temp, &pList[j], sizeof(ActiveMotor_t));
				memcpy(&pList[j], &pList[i], sizeof(ActiveMotor_t));
				memcpy(&pList[i], &temp, sizeof(ActiveMotor_t));
			}
		}
	}
	//定义对应的端口
	for(uint8_t i = 0; i < count; i++)
	{
		if(pList[i].busGroup == MOTOR_BUS_GROUP_0)
		{
			pList[i].index = index;
			index++;
		}
		else if(pList[i].busGroup == MOTOR_BUS_GROUP_1)
		{
			if(index < 2)
			{
				index = 2;
			}
			pList[i].index = index;
			index++;
		}
	}

	for(uint8_t i = 0; i < count; i++)
	{
		LOGD(TAG, "index%d\t%d\t%d", i, pList[i].index, pList[i].type);
	}
}


//博铭维电机控制入口
void command_to_bo_motor(Motor_t *motor, JointCtrl_t *joint)
{
	if(motor->isExist == FALSE)			//该电机没有连接上，那么不需要响应指令
	{
		return;
	}

    if(joint->type == CAN_PACKET_SET_NONE)
    {
        return;
    }
    else if(joint->type == CAN_PACKET_SET_IDLE)
    {
    	if(MOTOR_MODE_IDLE != motor_curMode_get(&motor->motor))
    	{
    		motor_mode_change(&motor->motor, MOTOR_MODE_IDLE);
    	}
    }
    else if(joint->type == CAN_PACKET_SET_CURRENT)
    {          // 电流环模式
        const float scale = 1000.0f;
        float torque = (float)joint->tff_des / scale;
        if(MOTOR_MODE_TORQUE == motor_curMode_get(&motor->motor))
        {
        	motor_torque_set(&motor->motor, torque);
        }
        else
        {
        	motor_mode_change(&motor->motor, MOTOR_MODE_TORQUE);
        }
//        LOGD(TAG, "motor%ld:CAN_PACKET_SET_CURRENT %ld", motor->canId, joint->tff_des);
    }
    else if(joint->type == CAN_PACKET_SET_RPM)
    {              // 转速模式
        const float scale = 1000.0f;
        float speed = (float)joint->v_des / scale;
    	if(MOTOR_MODE_RPM == motor_curMode_get(&motor->motor))
    	{
    		motor_speed_set(&motor->motor, speed);
    	}
    	else
    	{
    		motor_mode_change(&motor->motor, MOTOR_MODE_RPM);
    	}
//    	LOGD(TAG, "motor%ld:CAN_PACKET_SET_RPM %ld", motor->canId, joint->v_des);
    }
    else if(joint->type == CAN_PACKET_SET_POS)
    {               // 位置环模式
        const float scale = 1000.0f;
        float pos_deg = (float)joint->p_des / scale;
        if(MOTOR_MODE_POS == motor_curMode_get(&motor->motor))
        {
        	motor_position_set(&motor->motor, pos_deg);
        }
        else
        {
        	motor_mode_change(&motor->motor, MOTOR_MODE_POS);
        }
//        LOGD(TAG, "motor%ld:CAN_PACKET_SET_POS %ld", motor->canId, joint->p_des);
    }
}

//小米电机控制入口
void command_to_mi_motor(MiMotor_t *motor, JointCtrl_t *joint)
{
	if(motor->isExist == FALSE)			//该电机没有连接上，那么不需要响应指令
	{
		return;
	}

    if(joint->type == CAN_PACKET_SET_NONE)
    {
        return;
    }
    else if(joint->type == CAN_PACKET_SET_IDLE)
    {
    	if(MI_MOTOR_MODE_IDLE != mi_motor_curMode_get(&motor->motor))
    	{
    		mi_motor_mode_change(&motor->motor, MI_MOTOR_MODE_IDLE);
    	}
    }
    else if(joint->type == CAN_PACKET_SET_CURRENT)
    {          // 电流环模式
        const float scale = 1000.0;
        float torque = (float)joint->tff_des / scale;
        if(MI_MOTOR_MODE_TORQUE == mi_motor_curMode_get(&motor->motor))
        {
        	mi_motor_torque_set(&motor->motor, torque);
        }
        else
        {
        	mi_motor_mode_change(&motor->motor, MI_MOTOR_MODE_TORQUE);
        }
//        LOGD(TAG, "mi motor%lx:CAN_PACKET_SET_CURRENT %ld", motor->canId, joint->tff_des);
    }
    else if(joint->type == CAN_PACKET_SET_RPM)
    {              // 转速模式
        const float scale = 1000.0f;
        float speed = (float)joint->v_des / scale;
    	if(MI_MOTOR_MODE_RPM == mi_motor_curMode_get(&motor->motor))
    	{
    		mi_motor_speed_set(&motor->motor, speed);
    	}
    	else
    	{
    		mi_motor_mode_change(&motor->motor, MI_MOTOR_MODE_RPM);
    	}
//    	LOGD(TAG, "mi motor%lx:CAN_PACKET_SET_RPM %ld", motor->canId, joint->v_des);
    }
    else if(joint->type == CAN_PACKET_SET_POS)
    {               // 位置环模式
        const float scale = 1000.0f;
        float pos_deg = (float)joint->p_des / scale;
        if(MI_MOTOR_MODE_POS == mi_motor_curMode_get(&motor->motor))
        {
        	mi_motor_position_set(&motor->motor, pos_deg);
        }
        else
        {
        	mi_motor_mode_change(&motor->motor, MI_MOTOR_MODE_POS);
        }
//        LOGD(TAG, "motor%lx:CAN_PACKET_SET_POS %ld", motor->canId, joint->p_des);
    }
    else if(joint->type == CAN_PACKET_SET_POS_SPD)
    {
        const float scale = 1000.0f;
        float pos_deg = (float)joint->p_des / scale;
        float speed = (float)joint->v_des / scale;
        float kp = (float)joint->kp_des / scale;
        float ki = (float)joint->ki_des / scale;
        if(MI_MOTOR_MODE_RUN_CTRL == mi_motor_curMode_get(&motor->motor))
        {
//        	LOGD(TAG, "run:%ld\t%ld\t%ld\t%ld", joint->p_des, joint->v_des, joint->kp_des, joint->ki_des);
        	mi_motor_runctrl_param_set(&motor->motor, pos_deg, speed, kp, ki);
        }
        else
        {
        	mi_motor_mode_change(&motor->motor, MI_MOTOR_MODE_RUN_CTRL);
        }
    }
}

void command_to_motor(JointCtrl_t *joint, uint8_t motorId)
{
//	MotorUnit_t *motor = &g_tMotor[motorId].motor;
	uint8_t index;
	for(index = 0; index < g_cActiveMotorSum; index++)
	{
		if(g_tActiveMotor[index].index == motorId)
		{
			if(g_tActiveMotor[index].type == MOTOR_TYPE_BOMINGWEI)
			{
				command_to_bo_motor(g_tActiveMotor[index].pMotorHandle, joint);
			}
			else if(g_tActiveMotor[index].type == MOTOR_TYPE_XIAOMI)
			{
				command_to_mi_motor(g_tActiveMotor[index].pMotorHandle, joint);
			}
		}
	}
}

//扫描当前接入的电机,并将扫描到的电机句柄添加到已激活队列
int motor_count_update(void)
{
	osStatus_t osStatus;
	CIA402_STATE_e state;
	uint8_t devId[8];
	ActiveMotor_t active[MOTOR_SUM];
	uint8_t motorSum = 0;

	for(uint8_t i = 0; i < BO_MOTOR_NUM; i++)		//先检测电机是否已经正常连接
	{
		if(motor_state_get(&g_tMotor[i].motor, &state))
		{
			g_tMotor[i].isExist = FALSE;
			memset(&g_tMotor[i].info, 0, sizeof(g_tMotor[i].info));
		}
		else
		{
			g_tMotor[i].isExist = TRUE;
			active[motorSum].type = g_tMotor[i].type;
			active[motorSum].busGroup = g_tMotor[i].busGroup;
			active[motorSum].pMotorHandle = &g_tMotor[i];
			motorSum++;
			LOGD(TAG, "Found bo motor%d, id=%lx", i, g_tMotor[i].canId);
		}

		g_tMotor[i].lostCnt = 0;
	}

	for(uint8_t i = 0; i < MI_MOTOR_NUM; i++)
	{
		if(mi_query_motor_dev_id(&g_tMiMotor[i].motor, devId))
		{
			g_tMiMotor[i].isExist = FALSE;
			memset(&g_tMiMotor[i].info, 0, sizeof(g_tMiMotor[i].info));
		}
		else
		{
			g_tMiMotor[i].isExist = TRUE;
			active[motorSum].type = g_tMiMotor[i].type;
			active[motorSum].busGroup = g_tMiMotor[i].busGroup;
			active[motorSum].pMotorHandle = &g_tMiMotor[i];
			motorSum++;
			LOGD(TAG, "Found xiaomi motor%d, id=%lx", i, g_tMiMotor[i].canId);
		}
	}

	if(motorSum > 0)
	{
		//排序并定义端口口
		active_motor_sort(active, motorSum);
		//同步
		osStatus = osMutexAcquire(g_tActiveMotorMutex, 10);
		if(osStatus)
		{
			return osStatus;
		}
		memcpy(g_tActiveMotor, active, sizeof(ActiveMotor_t) * motorSum);
		g_cActiveMotorSum = motorSum;
		osMutexRelease(g_tActiveMotorMutex);
	}
	else
	{
		g_cActiveMotorSum = 0;
	}

	LOGD(TAG, "Found motor count=%d", g_cActiveMotorSum);

	return g_cActiveMotorSum;
}

void motor_task(void *param)
{
	uint8_t aMsg[2];
	osStatus_t osStatus;
	JointCtrl_t *pJointCtrl;

	osDelay(500);

	can1_config(NULL);
	can2_config(NULL);

	if(motor_count_update())			//该芯片控制电机
	{
		LOGD(TAG, "Found motor");
	}
	else					//该芯片连接拉力传感器
	{
		LOGD(TAG, "Can not found motor");
	}

	while(1)
	{
		osStatus = osMessageQueueGet(g_tMotorQueue, aMsg, NULL, 100);
		if(osStatus)
		{
			motor_info_get();
			continue;
		}

		switch(aMsg[0])
		{
		case MOTOR_MSG_GET_INFO:
			motor_info_get();
			break;

		case MOTOR_MSG_UPDATE_DEV:
			motor_count_update();
			break;

		case MOTOR_MSG_SEND_COMMAND:
			pJointCtrl = joint_command_get();
			for(uint8_t i = 0; i < JOINT_NUM; i++)
			{
				command_to_motor(&pJointCtrl[i], i);
			}
			break;

		case MOTOR_MSG_HOST_LOST:
			joint_command_clear();
			pJointCtrl = joint_command_get();
			for(uint8_t i = 0; i < JOINT_NUM; i++)
			{
				command_to_motor(&pJointCtrl[i], i);
			}
			break;

		default:
			break;

		}
	}
}

int motor_task_init(void)
{
	g_tMotorQueue = osMessageQueueNew (8, sizeof(uint8_t)*2, &g_tMotorQueueAttr);
	if(g_tMotorQueue == NULL)
	{
		LOGE(TAG, "motor task queue create error!!!");
		return -1;
	}

	g_tActiveMotorMutex = osMutexNew(&g_tActiveMotorMutexAttr);
	if(g_tActiveMotorMutex == NULL)
	{
		LOGE(TAG, "Active motor mutex create error");
		return -1;
	}

	for(uint8_t i = 0; i < BO_MOTOR_NUM; i++)		//先检测电机是否已经正常连接
	{
		motor_init(&g_tMotor[i].motor, g_tMotor[i].canId, &g_tMotorAttr);
	}

	for(uint8_t i = 0; i < MI_MOTOR_NUM; i++)		//先检测电机是否已经正常连接
	{
		mi_motor_init(&g_tMiMotor[i].motor, g_tMiMotor[i].canId, &g_tMiMotorAttr);
	}

	g_tMotorTaskHandle = osThreadNew(motor_task, NULL, &g_tMotorTaskAttr);
	if(g_tMotorTaskHandle == NULL)
	{
		LOGE(TAG, "Motor task create error!!!");
		return -1;
	}
	LOGD(TAG, "motor task init ok");

	return 0;
}
